import rclpy
from rclpy.node import Node
from gazebo_msgs.srv import SpawnEntity
from geometry_msgs.msg import Pose
import os
from ament_index_python.packages import get_package_share_directory

class ModelSpawner(Node):
    def __init__(self):
        super().__init__('model_spawner')
        self.client = self.create_client(SpawnEntity, '/spawn_entity')  # Humble中服务名固定为/spawn_entity
        while not self.client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('等待spawn_entity服务...')
        
    def spawn_model(self, model_name, model_xml, pose):
        req = SpawnEntity.Request()
        req.name = model_name
        req.xml = model_xml
        req.initial_pose = pose
        req.reference_frame = "world"
        future = self.client.call_async(req)
        rclpy.spin_until_future_complete(self, future)
        if future.result() is not None:
            self.get_logger().info(f'模型 {model_name} 生成成功!')
        else:
            self.get_logger().error(f'生成模型失败: {future.exception()}')

def main():
    rclpy.init()
    spawner = ModelSpawner()
    
    # 从功能包内读取模型文件（推荐方式）
    pkg_path = get_package_share_directory('spawn_model_pkg')
    model_path = os.path.join(pkg_path, 'models', 'model.sdf')  # 模型文件放在功能包的models目录下
    
    try:
        with open(model_path, 'r') as f:
            xml = f.read()
    except FileNotFoundError:
        spawner.get_logger().error(f"模型文件未找到: {model_path}")
        return
    
    initial_pose = Pose()
    initial_pose.position.x = 1.0
    initial_pose.position.y = 0.5
    initial_pose.position.z = 0.5
    initial_pose.orientation.w = 1.0
    
    spawner.spawn_model('dynamic_cylinder', xml, initial_pose)
    rclpy.shutdown()

if __name__ == '__main__':
    main()
